home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2004 April / Gamestar_61_2004-04_dvdb.iso / DVDStar / Editace / hltp.exe / {app} / Source Code / Half-Life Model Viewer / src / mathlib.c < prev    next >
C/C++ Source or Header  |  1999-05-04  |  8KB  |  360 lines
  1. /***
  2. *
  3. *    Copyright (c) 1998, Valve LLC. All rights reserved.
  4. *    
  5. *    This product contains software technology licensed from Id 
  6. *    Software, Inc. ("Id Technology").  Id Technology (c) 1996 Id Software, Inc. 
  7. *    All Rights Reserved.
  8. *
  9. ****/
  10.  
  11. // mathlib.c -- math primitives
  12.  
  13. #pragma warning( disable : 4244 )
  14. #pragma warning( disable : 4237 )
  15. #pragma warning( disable : 4305 )
  16.  
  17.  
  18. /* #include "cmdlib.h" */
  19. #ifndef true
  20. #define true 1
  21. #endif /* true */
  22. #ifndef false
  23. #define false 0
  24. #endif /* false */
  25.  
  26. #include "mathlib.h"
  27.  
  28. vec3_t vec3_origin = {0,0,0};
  29.  
  30.  
  31. double VectorLength(vec3_t v)
  32. {
  33.     int        i;
  34.     double    length;
  35.     
  36.     length = 0;
  37.     for (i=0 ; i< 3 ; i++)
  38.         length += v[i]*v[i];
  39.     length = sqrt (length);        // FIXME
  40.  
  41.     return length;
  42. }
  43.  
  44.  
  45. int VectorCompare (vec3_t v1, vec3_t v2)
  46. {
  47.     int        i;
  48.     
  49.     for (i=0 ; i<3 ; i++)
  50.         if (fabs(v1[i]-v2[i]) > EQUAL_EPSILON)
  51.             return false;
  52.             
  53.     return true;
  54. }
  55.  
  56. vec_t Q_rint (vec_t in)
  57. {
  58.     return floor (in + 0.5);
  59. }
  60.  
  61. void VectorMA (vec3_t va, double scale, vec3_t vb, vec3_t vc)
  62. {
  63.     vc[0] = va[0] + scale*vb[0];
  64.     vc[1] = va[1] + scale*vb[1];
  65.     vc[2] = va[2] + scale*vb[2];
  66. }
  67.  
  68. void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
  69. {
  70.     cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
  71.     cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
  72.     cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
  73. }
  74.  
  75. vec_t _DotProduct (vec3_t v1, vec3_t v2)
  76. {
  77.     return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
  78. }
  79.  
  80. void _VectorSubtract (vec3_t va, vec3_t vb, vec3_t out)
  81. {
  82.     out[0] = va[0]-vb[0];
  83.     out[1] = va[1]-vb[1];
  84.     out[2] = va[2]-vb[2];
  85. }
  86.  
  87. void _VectorAdd (vec3_t va, vec3_t vb, vec3_t out)
  88. {
  89.     out[0] = va[0]+vb[0];
  90.     out[1] = va[1]+vb[1];
  91.     out[2] = va[2]+vb[2];
  92. }
  93.  
  94. void _VectorCopy (vec3_t in, vec3_t out)
  95. {
  96.     out[0] = in[0];
  97.     out[1] = in[1];
  98.     out[2] = in[2];
  99. }
  100.  
  101. void _VectorScale (vec3_t v, vec_t scale, vec3_t out)
  102. {
  103.     out[0] = v[0] * scale;
  104.     out[1] = v[1] * scale;
  105.     out[2] = v[2] * scale;
  106. }
  107.  
  108. vec_t VectorNormalize (vec3_t v)
  109. {
  110.     int        i;
  111.     double    length;
  112.  
  113. if ( fabs(v[1] - 0.000215956) < 0.0001)
  114. i=1;
  115.  
  116.     length = 0;
  117.     for (i=0 ; i< 3 ; i++)
  118.         length += v[i]*v[i];
  119.     length = sqrt (length);
  120.     if (length == 0)
  121.         return 0;
  122.         
  123.     for (i=0 ; i< 3 ; i++)
  124.         v[i] /= length;    
  125.  
  126.     return length;
  127. }
  128.  
  129. void VectorInverse (vec3_t v)
  130. {
  131.     v[0] = -v[0];
  132.     v[1] = -v[1];
  133.     v[2] = -v[2];
  134. }
  135.  
  136. void ClearBounds (vec3_t mins, vec3_t maxs)
  137. {
  138.     mins[0] = mins[1] = mins[2] = 99999;
  139.     maxs[0] = maxs[1] = maxs[2] = -99999;
  140. }
  141.  
  142. void AddPointToBounds (vec3_t v, vec3_t mins, vec3_t maxs)
  143. {
  144.     int        i;
  145.     vec_t    val;
  146.  
  147.     for (i=0 ; i<3 ; i++)
  148.     {
  149.         val = v[i];
  150.         if (val < mins[i])
  151.             mins[i] = val;
  152.         if (val > maxs[i])
  153.             maxs[i] = val;
  154.     }
  155. }
  156.  
  157. void AngleMatrix (const vec3_t angles, float (*matrix)[4] )
  158. {
  159.     float        angle;
  160.     float        sr, sp, sy, cr, cp, cy;
  161.     
  162.     angle = angles[2] * (Q_PI*2 / 360);
  163.     sy = sin(angle);
  164.     cy = cos(angle);
  165.     angle = angles[1] * (Q_PI*2 / 360);
  166.     sp = sin(angle);
  167.     cp = cos(angle);
  168.     angle = angles[0] * (Q_PI*2 / 360);
  169.     sr = sin(angle);
  170.     cr = cos(angle);
  171.  
  172.     // matrix = (Z * Y) * X
  173.     matrix[0][0] = cp*cy;
  174.     matrix[1][0] = cp*sy;
  175.     matrix[2][0] = -sp;
  176.     matrix[0][1] = sr*sp*cy+cr*-sy;
  177.     matrix[1][1] = sr*sp*sy+cr*cy;
  178.     matrix[2][1] = sr*cp;
  179.     matrix[0][2] = (cr*sp*cy+-sr*-sy);
  180.     matrix[1][2] = (cr*sp*sy+-sr*cy);
  181.     matrix[2][2] = cr*cp;
  182.     matrix[0][3] = 0.0;
  183.     matrix[1][3] = 0.0;
  184.     matrix[2][3] = 0.0;
  185. }
  186.  
  187. void AngleIMatrix (const vec3_t angles, float matrix[3][4] )
  188. {
  189.     float        angle;
  190.     float        sr, sp, sy, cr, cp, cy;
  191.     
  192.     angle = angles[2] * (Q_PI*2 / 360);
  193.     sy = sin(angle);
  194.     cy = cos(angle);
  195.     angle = angles[1] * (Q_PI*2 / 360);
  196.     sp = sin(angle);
  197.     cp = cos(angle);
  198.     angle = angles[0] * (Q_PI*2 / 360);
  199.     sr = sin(angle);
  200.     cr = cos(angle);
  201.  
  202.     // matrix = (Z * Y) * X
  203.     matrix[0][0] = cp*cy;
  204.     matrix[0][1] = cp*sy;
  205.     matrix[0][2] = -sp;
  206.     matrix[1][0] = sr*sp*cy+cr*-sy;
  207.     matrix[1][1] = sr*sp*sy+cr*cy;
  208.     matrix[1][2] = sr*cp;
  209.     matrix[2][0] = (cr*sp*cy+-sr*-sy);
  210.     matrix[2][1] = (cr*sp*sy+-sr*cy);
  211.     matrix[2][2] = cr*cp;
  212.     matrix[0][3] = 0.0;
  213.     matrix[1][3] = 0.0;
  214.     matrix[2][3] = 0.0;
  215. }
  216.  
  217. void R_ConcatTransforms (const float in1[3][4], const float in2[3][4], float out[3][4])
  218. {
  219.     out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
  220.                 in1[0][2] * in2[2][0];
  221.     out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
  222.                 in1[0][2] * in2[2][1];
  223.     out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
  224.                 in1[0][2] * in2[2][2];
  225.     out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
  226.                 in1[0][2] * in2[2][3] + in1[0][3];
  227.     out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
  228.                 in1[1][2] * in2[2][0];
  229.     out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
  230.                 in1[1][2] * in2[2][1];
  231.     out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
  232.                 in1[1][2] * in2[2][2];
  233.     out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
  234.                 in1[1][2] * in2[2][3] + in1[1][3];
  235.     out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
  236.                 in1[2][2] * in2[2][0];
  237.     out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
  238.                 in1[2][2] * in2[2][1];
  239.     out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
  240.                 in1[2][2] * in2[2][2];
  241.     out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
  242.                 in1[2][2] * in2[2][3] + in1[2][3];
  243. }
  244.  
  245.  
  246.  
  247. void VectorRotate (const vec3_t in1, const float in2[3][4], vec3_t out)
  248. {
  249.     out[0] = DotProduct(in1, in2[0]);
  250.     out[1] = DotProduct(in1, in2[1]);
  251.     out[2] = DotProduct(in1, in2[2]);
  252. }
  253.  
  254.  
  255. // rotate by the inverse of the matrix
  256. void VectorIRotate (const vec3_t in1, const float in2[3][4], vec3_t out)
  257. {
  258.     out[0] = in1[0]*in2[0][0] + in1[1]*in2[1][0] + in1[2]*in2[2][0];
  259.     out[1] = in1[0]*in2[0][1] + in1[1]*in2[1][1] + in1[2]*in2[2][1];
  260.     out[2] = in1[0]*in2[0][2] + in1[1]*in2[1][2] + in1[2]*in2[2][2];
  261. }
  262.  
  263.  
  264. void VectorTransform (const vec3_t in1, const float in2[3][4], vec3_t out)
  265. {
  266.     out[0] = DotProduct(in1, in2[0]) + in2[0][3];
  267.     out[1] = DotProduct(in1, in2[1]) +    in2[1][3];
  268.     out[2] = DotProduct(in1, in2[2]) +    in2[2][3];
  269. }
  270.  
  271.  
  272.  
  273. void AngleQuaternion( const vec3_t angles, vec4_t quaternion )
  274. {
  275.     float        angle;
  276.     float        sr, sp, sy, cr, cp, cy;
  277.  
  278.     // FIXME: rescale the inputs to 1/2 angle
  279.     angle = angles[2] * 0.5;
  280.     sy = sin(angle);
  281.     cy = cos(angle);
  282.     angle = angles[1] * 0.5;
  283.     sp = sin(angle);
  284.     cp = cos(angle);
  285.     angle = angles[0] * 0.5;
  286.     sr = sin(angle);
  287.     cr = cos(angle);
  288.  
  289.     quaternion[0] = sr*cp*cy-cr*sp*sy; // X
  290.     quaternion[1] = cr*sp*cy+sr*cp*sy; // Y
  291.     quaternion[2] = cr*cp*sy-sr*sp*cy; // Z
  292.     quaternion[3] = cr*cp*cy+sr*sp*sy; // W
  293. }
  294.  
  295. void QuaternionMatrix( const vec4_t quaternion, float (*matrix)[4] )
  296. {
  297.  
  298.     matrix[0][0] = 1.0 - 2.0 * quaternion[1] * quaternion[1] - 2.0 * quaternion[2] * quaternion[2];
  299.     matrix[1][0] = 2.0 * quaternion[0] * quaternion[1] + 2.0 * quaternion[3] * quaternion[2];
  300.     matrix[2][0] = 2.0 * quaternion[0] * quaternion[2] - 2.0 * quaternion[3] * quaternion[1];
  301.  
  302.     matrix[0][1] = 2.0 * quaternion[0] * quaternion[1] - 2.0 * quaternion[3] * quaternion[2];
  303.     matrix[1][1] = 1.0 - 2.0 * quaternion[0] * quaternion[0] - 2.0 * quaternion[2] * quaternion[2];
  304.     matrix[2][1] = 2.0 * quaternion[1] * quaternion[2] + 2.0 * quaternion[3] * quaternion[0];
  305.  
  306.     matrix[0][2] = 2.0 * quaternion[0] * quaternion[2] + 2.0 * quaternion[3] * quaternion[1];
  307.     matrix[1][2] = 2.0 * quaternion[1] * quaternion[2] - 2.0 * quaternion[3] * quaternion[0];
  308.     matrix[2][2] = 1.0 - 2.0 * quaternion[0] * quaternion[0] - 2.0 * quaternion[1] * quaternion[1];
  309. }
  310.  
  311. void QuaternionSlerp( const vec4_t p, vec4_t q, float t, vec4_t qt )
  312. {
  313.     int i;
  314.     float omega, cosom, sinom, sclp, sclq;
  315.  
  316.     // decide if one of the quaternions is backwards
  317.     float a = 0;
  318.     float b = 0;
  319.     for (i = 0; i < 4; i++) {
  320.         a += (p[i]-q[i])*(p[i]-q[i]);
  321.         b += (p[i]+q[i])*(p[i]+q[i]);
  322.     }
  323.     if (a > b) {
  324.         for (i = 0; i < 4; i++) {
  325.             q[i] = -q[i];
  326.         }
  327.     }
  328.  
  329.     cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];
  330.  
  331.     if ((1.0 + cosom) > 0.00000001) {
  332.         if ((1.0 - cosom) > 0.00000001) {
  333.             omega = acos( cosom );
  334.             sinom = sin( omega );
  335.             sclp = sin( (1.0 - t)*omega) / sinom;
  336.             sclq = sin( t*omega ) / sinom;
  337.         }
  338.         else {
  339.             sclp = 1.0 - t;
  340.             sclq = t;
  341.         }
  342.         for (i = 0; i < 4; i++) {
  343.             qt[i] = sclp * p[i] + sclq * q[i];
  344.         }
  345.     }
  346.     else {
  347.         qt[0] = -p[1];
  348.         qt[1] = p[0];
  349.         qt[2] = -p[3];
  350.         qt[3] = p[2];
  351.         sclp = sin( (1.0 - t) * 0.5 * Q_PI);
  352.         sclq = sin( t * 0.5 * Q_PI);
  353.         for (i = 0; i < 3; i++) {
  354.             qt[i] = sclp * p[i] + sclq * qt[i];
  355.         }
  356.     }
  357. }
  358.  
  359.  
  360.